#!/usr/bin/env python
#-*- coding: utf-8 -*-

import numpy as np

import pat.vehicles.fixed_wing.dynamic_model_python_basic as dm
import pat.utils as pu

# loads and displays the dynamic model's parameters
filename = "../config/Rcam.xml"
p_dm = dm.Param(filename)
print p_dm,"\n"

# finds trim conditions
args = {'va':70,  'gamma':pu.rad_of_deg(-5.), 'h':0}
Xe, Ue = dm.trim(p_dm, args, debug=True)
print ""
# finds another trim conditions
#args = {'va':70,  'gamma':pu.rad_of_deg(-5.), 'h':0}
#va=70; throttle=0.3
#Xe, Ue = ctl.get_trim_cst_throttle(va, throttle, p_dm, debug=True)

#print Xe, Ue

A,B = pu.num_jacobian(Xe, Ue, p_dm, dm.dyn)

#print A,"\n",B

w, M = np.linalg.eig(A)

#print w,"\n", M
print ""


if 0:
    Vs = [50., 60., 70., 80., 90., 100., 110.]
    Eqs1 = [ctl.get_trim_cst_path(v, 0., p_dm, h=0.,     debug=True) for v in Vs]
    Eqs2 = [ctl.get_trim_cst_path(v, 0., p_dm, h=-5000., debug=True) for v in Vs]
    print Eqs1
    print Eqs2

if 0:
    filenames = ["../config/epicea_ref.xml",
                 "../config/epicea_ele_small.xml",
                 "../config/epicea_ele_large.xml",
                 "../config/epicea_rud_small.xml",
                 "../config/epicea_wing_moved.xml" ]
    for f in filenames:
        p_dm = dm.Param(f)
        #        print p_dm,"\n"
        # finds trim conditions
        va=70; gamma=pu.rad_of_deg(0.)
        Xe, Ue = ctl.get_trim_cst_path(va, gamma, p_dm, debug=False)
        A,B = pu.num_jacobian(Xe, Ue, p_dm, dm.dyn)
        pu.print_lti_dynamics(A, B, txt="open loop dynamics {:s}".format(p_dm.name), print_original_form=False, print_modal_form=False)
         
if __name__ == "__main__":
    pass
